home *** CD-ROM | disk | FTP | other *** search
/ BCI NET 2 / BCI NET 2.iso / archives / programming / source / graphicgems4.lha / GemsIV / polar_decomp / Decompose.c next >
Encoding:
C/C++ Source or Header  |  1995-02-06  |  15.3 KB  |  502 lines
  1. /**** Decompose.c ****/
  2. /* Ken Shoemake, 1993 */
  3. #include <math.h>
  4. #include "Decompose.h"
  5.  
  6. /******* Matrix Preliminaries *******/
  7.  
  8. /** Fill out 3x3 matrix to 4x4 **/
  9. #define mat_pad(A) (A[W][X]=A[X][W]=A[W][Y]=A[Y][W]=A[W][Z]=A[Z][W]=0,A[W][W]=1)
  10.  
  11. /** Copy nxn matrix A to C using "gets" for assignment **/
  12. #define mat_copy(C,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
  13.     C[i][j] gets (A[i][j]);}
  14.  
  15. /** Copy transpose of nxn matrix A to C using "gets" for assignment **/
  16. #define mat_tpose(AT,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
  17.     AT[i][j] gets (A[j][i]);}
  18.  
  19. /** Assign nxn matrix C the element-wise combination of A and B using "op" **/
  20. #define mat_binop(C,gets,A,op,B,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
  21.     C[i][j] gets (A[i][j]) op (B[i][j]);}
  22.  
  23. /** Multiply the upper left 3x3 parts of A and B to get AB **/
  24. void mat_mult(HMatrix A, HMatrix B, HMatrix AB)
  25. {
  26.     int i, j;
  27.     for (i=0; i<3; i++) for (j=0; j<3; j++)
  28.     AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
  29. }
  30.  
  31. /** Return dot product of length 3 vectors va and vb **/
  32. float vdot(float *va, float *vb)
  33. {
  34.     return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]);
  35. }
  36.  
  37. /** Set v to cross product of length 3 vectors va and vb **/
  38. void vcross(float *va, float *vb, float *v)
  39. {
  40.     v[0] = va[1]*vb[2] - va[2]*vb[1];
  41.     v[1] = va[2]*vb[0] - va[0]*vb[2];
  42.     v[2] = va[0]*vb[1] - va[1]*vb[0];
  43. }
  44.  
  45. /** Set MadjT to transpose of inverse of M times determinant of M **/
  46. void adjoint_transpose(HMatrix M, HMatrix MadjT)
  47. {
  48.     vcross(M[1], M[2], MadjT[0]);
  49.     vcross(M[2], M[0], MadjT[1]);
  50.     vcross(M[0], M[1], MadjT[2]);
  51. }
  52.  
  53. /******* Quaternion Preliminaries *******/
  54.  
  55. /* Construct a (possibly non-unit) quaternion from real components. */
  56. Quat Qt_(float x, float y, float z, float w)
  57. {
  58.     Quat qq;
  59.     qq.x = x; qq.y = y; qq.z = z; qq.w = w;
  60.     return (qq);
  61. }
  62.  
  63. /* Return conjugate of quaternion. */
  64. Quat Qt_Conj(Quat q)
  65. {
  66.     Quat qq;
  67.     qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
  68.     return (qq);
  69. }
  70.  
  71. /* Return quaternion product qL * qR.  Note: order is important!
  72.  * To combine rotations, use the product Mul(qSecond, qFirst),
  73.  * which gives the effect of rotating by qFirst then qSecond. */
  74. Quat Qt_Mul(Quat qL, Quat qR)
  75. {
  76.     Quat qq;
  77.     qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
  78.     qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
  79.     qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
  80.     qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
  81.     return (qq);
  82. }
  83.  
  84. /* Return product of quaternion q by scalar w. */
  85. Quat Qt_Scale(Quat q, float w)
  86. {
  87.     Quat qq;
  88.     qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w;
  89.     return (qq);
  90. }
  91.  
  92. /* Construct a unit quaternion from rotation matrix.  Assumes matrix is
  93.  * used to multiply column vector on the left: vnew = mat vold.     Works
  94.  * correctly for right-handed coordinate system and right-handed rotations.
  95.  * Translation and perspective components ignored. */
  96. Quat Qt_FromMatrix(HMatrix mat)
  97. {
  98.     /* This algorithm avoids near-zero divides by looking for a large component
  99.      * - first w, then x, y, or z.  When the trace is greater than zero,
  100.      * |w| is greater than 1/2, which is as small as a largest component can be.
  101.      * Otherwise, the largest diagonal entry corresponds to the largest of |x|,
  102.      * |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
  103.     Quat qu;
  104.     register double tr, s;
  105.  
  106.     tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z];
  107.     if (tr >= 0.0) {
  108.         s = sqrt(tr + mat[W][W]);
  109.         qu.w = s*0.5;
  110.         s = 0.5 / s;
  111.         qu.x = (mat[Z][Y] - mat[Y][Z]) * s;
  112.         qu.y = (mat[X][Z] - mat[Z][X]) * s;
  113.         qu.z = (mat[Y][X] - mat[X][Y]) * s;
  114.     } else {
  115.         int h = X;
  116.         if (mat[Y][Y] > mat[X][X]) h = Y;
  117.         if (mat[Z][Z] > mat[h][h]) h = Z;
  118.         switch (h) {
  119. #define caseMacro(i,j,k,I,J,K) \
  120.         case I:\
  121.         s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\
  122.         qu.i = s*0.5;\
  123.         s = 0.5 / s;\
  124.         qu.j = (mat[I][J] + mat[J][I]) * s;\
  125.         qu.k = (mat[K][I] + mat[I][K]) * s;\
  126.         qu.w = (mat[K][J] - mat[J][K]) * s;\
  127.         break
  128.         caseMacro(x,y,z,X,Y,Z);
  129.         caseMacro(y,z,x,Y,Z,X);
  130.         caseMacro(z,x,y,Z,X,Y);
  131.         }
  132.     }
  133.     if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1/sqrt(mat[W][W]));
  134.     return (qu);
  135. }
  136. /******* Decomp Auxiliaries *******/
  137.  
  138. static HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
  139.  
  140. /** Compute either the 1 or infinity norm of M, depending on tpose **/
  141. float mat_norm(HMatrix M, int tpose)
  142. {
  143.     int i;
  144.     float sum, max;
  145.     max = 0.0;
  146.     for (i=0; i<3; i++) {
  147.     if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]);
  148.     else       sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]);
  149.     if (max<sum) max = sum;
  150.     }
  151.     return max;
  152. }
  153.  
  154. float norm_inf(HMatrix M) {mat_norm(M, 0);}
  155. float norm_one(HMatrix M) {mat_norm(M, 1);}
  156.  
  157. /** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
  158. int find_max_col(HMatrix M)
  159. {
  160.     float abs, max;
  161.     int i, j, col;
  162.     max = 0.0; col = -1;
  163.     for (i=0; i<3; i++) for (j=0; j<3; j++) {
  164.     abs = M[i][j]; if (abs<0.0) abs = -abs;
  165.     if (abs>max) {max = abs; col = j;}
  166.     }
  167.     return col;
  168. }
  169.  
  170. /** Setup u for Household reflection to zero all v components but first **/
  171. void make_reflector(float *v, float *u)
  172. {
  173.     float s = sqrt(vdot(v, v));
  174.     u[0] = v[0]; u[1] = v[1];
  175.     u[2] = v[2] + ((v[2]<0.0) ? -s : s);
  176.     s = sqrt(2.0/vdot(u, u));
  177.     u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s;
  178. }
  179.  
  180. /** Apply Householder reflection represented by u to column vectors of M **/
  181. void reflect_cols(HMatrix M, float *u)
  182. {
  183.     int i, j;
  184.     for (i=0; i<3; i++) {
  185.     float s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i];
  186.     for (j=0; j<3; j++) M[j][i] -= u[j]*s;
  187.     }
  188. }
  189. /** Apply Householder reflection represented by u to row vectors of M **/
  190. void reflect_rows(HMatrix M, float *u)
  191. {
  192.     int i, j;
  193.     for (i=0; i<3; i++) {
  194.     float s = vdot(u, M[i]);
  195.     for (j=0; j<3; j++) M[i][j] -= u[j]*s;
  196.     }
  197. }
  198.  
  199. /** Find orthogonal factor Q of rank 1 (or less) M **/
  200. void do_rank1(HMatrix M, HMatrix Q)
  201. {
  202.     float v1[3], v2[3], s;
  203.     int col;
  204.     mat_copy(Q,=,mat_id,4);
  205.     /* If rank(M) is 1, we should find a non-zero column in M */
  206.     col = find_max_col(M);
  207.     if (col<0) return; /* Rank is 0 */
  208.     v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
  209.     make_reflector(v1, v1); reflect_cols(M, v1);
  210.     v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
  211.     make_reflector(v2, v2); reflect_rows(M, v2);
  212.     s = M[2][2];
  213.     if (s<0.0) Q[2][2] = -1.0;
  214.     reflect_cols(Q, v1); reflect_rows(Q, v2);
  215. }
  216.  
  217. /** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/
  218. void do_rank2(HMatrix M, HMatrix MadjT, HMatrix Q)
  219. {
  220.     float v1[3], v2[3];
  221.     float w, x, y, z, c, s, d;
  222.     int i, j, col;
  223.     /* If rank(M) is 2, we should find a non-zero column in MadjT */
  224.     col = find_max_col(MadjT);
  225.     if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */
  226.     v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col];
  227.     make_reflector(v1, v1); reflect_cols(M, v1);
  228.     vcross(M[0], M[1], v2);
  229.     make_reflector(v2, v2); reflect_rows(M, v2);
  230.     w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1];
  231.     if (w*z>x*y) {
  232.     c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
  233.     Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s);
  234.     } else {
  235.     c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
  236.     Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s;
  237.     }
  238.     Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0;
  239.     reflect_cols(Q, v1); reflect_rows(Q, v2);
  240. }
  241.  
  242.  
  243. /******* Polar Decomposition *******/
  244.  
  245. /* Polar Decomposition of 3x3 matrix in 4x4,
  246.  * M = QS.  See Nicholas Higham and Robert S. Schreiber,
  247.  * Fast Polar Decomposition of An Arbitrary Matrix,
  248.  * Technical Report 88-942, October 1988,
  249.  * Department of Computer Science, Cornell University.
  250.  */
  251. float polar_decomp(HMatrix M, HMatrix Q, HMatrix S)
  252. {
  253. #define TOL 1.0e-6
  254.     HMatrix Mk, MadjTk, Ek;
  255.     float det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, t1, t2, g1, g2;
  256.     int i, j;
  257.     mat_tpose(Mk,=,M,3);
  258.     M_one = norm_one(Mk);  M_inf = norm_inf(Mk);
  259.     do {
  260.     adjoint_transpose(Mk, MadjTk);
  261.     det = vdot(Mk[0], MadjTk[0]);
  262.     if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;}
  263.     MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk);
  264.     gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det));
  265.     g1 = gamma*0.5;
  266.     g2 = 0.5/(gamma*det);
  267.     mat_copy(Ek,=,Mk,3);
  268.     mat_binop(Mk,=,g1*Mk,+,g2*MadjTk,3);
  269.     mat_copy(Ek,-=,Mk,3);
  270.     E_one = norm_one(Ek);
  271.     M_one = norm_one(Mk);  M_inf = norm_inf(Mk);
  272.     } while (E_one>(M_one*TOL));
  273.     mat_tpose(Q,=,Mk,3); mat_pad(Q);
  274.     mat_mult(Mk, M, S);     mat_pad(S);
  275.     for (i=0; i<3; i++) for (j=i; j<3; j++)
  276.     S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]);
  277.     return (det);
  278. }
  279.  
  280.  
  281.  
  282.  
  283.  
  284.  
  285.  
  286.  
  287.  
  288.  
  289.  
  290.  
  291.  
  292.  
  293.  
  294.  
  295.  
  296. /******* Spectral Decomposition *******/
  297.  
  298. /* Compute the spectral decomposition of symmetric positive semi-definite S.
  299.  * Returns rotation in U and scale factors in result, so that if K is a diagonal
  300.  * matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method.
  301.  * See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983.
  302.  */
  303. HVect spect_decomp(HMatrix S, HMatrix U)
  304. {
  305.     HVect kv;
  306.     double Diag[3],OffD[3]; /* OffD is off-diag (by omitted index) */
  307.     double g,h,fabsh,fabsOffDi,t,theta,c,s,tau,ta,OffDq,a,b;
  308.     static char nxt[] = {Y,Z,X};
  309.     int sweep, i, j;
  310.     mat_copy(U,=,mat_id,4);
  311.     Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z];
  312.     OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y];
  313.     for (sweep=20; sweep>0; sweep--) {
  314.     float sm = fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z]);
  315.     if (sm==0.0) break;
  316.     for (i=Z; i>=X; i--) {
  317.         int p = nxt[i]; int q = nxt[p];
  318.         fabsOffDi = fabs(OffD[i]);
  319.         g = 100.0*fabsOffDi;
  320.         if (fabsOffDi>0.0) {
  321.         h = Diag[q] - Diag[p];
  322.         fabsh = fabs(h);
  323.         if (fabsh+g==fabsh) {
  324.             t = OffD[i]/h;
  325.         } else {
  326.             theta = 0.5*h/OffD[i];
  327.             t = 1.0/(fabs(theta)+sqrt(theta*theta+1.0));
  328.             if (theta<0.0) t = -t;
  329.         }
  330.         c = 1.0/sqrt(t*t+1.0); s = t*c;
  331.         tau = s/(c+1.0);
  332.         ta = t*OffD[i]; OffD[i] = 0.0;
  333.         Diag[p] -= ta; Diag[q] += ta;
  334.         OffDq = OffD[q];
  335.         OffD[q] -= s*(OffD[p] + tau*OffD[q]);
  336.         OffD[p] += s*(OffDq   - tau*OffD[p]);
  337.         for (j=Z; j>=X; j--) {
  338.             a = U[j][p]; b = U[j][q];
  339.             U[j][p] -= s*(b + tau*a);
  340.             U[j][q] += s*(a - tau*b);
  341.         }
  342.         }
  343.     }
  344.     }
  345.     kv.x = Diag[X]; kv.y = Diag[Y]; kv.z = Diag[Z]; kv.w = 1.0;
  346.     return (kv);
  347. }
  348.  
  349. /******* Spectral Axis Adjustment *******/
  350.  
  351. /* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p,
  352.  * which permutes the axes and turns freely in the plane of duplicate scale
  353.  * factors, such that q p has the largest possible w component, i.e. the
  354.  * smallest possible angle. Permutes k's components to go with q p instead of q.
  355.  * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
  356.  * Proceedings of Graphics Interface 1992. Details on p. 262-263.
  357.  */
  358. Quat snuggle(Quat q, HVect *k)
  359. {
  360. #define SQRTHALF (0.7071067811865475244)
  361. #define sgn(n,v)    ((n)?-(v):(v))
  362. #define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}
  363. #define cycle(a,p)  if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\
  364.             else   {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];}
  365.     Quat p;
  366.     float ka[4];
  367.     int i, turn = -1;
  368.     ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z;
  369.     if (ka[X]==ka[Y]) {if (ka[X]==ka[Z]) turn = W; else turn = Z;}
  370.     else {if (ka[X]==ka[Z]) turn = Y; else if (ka[Y]==ka[Z]) turn = X;}
  371.     if (turn>=0) {
  372.     Quat qtoz, qp;
  373.     unsigned neg[3], win;
  374.     double mag[3], c, s, t;
  375.     static Quat qxtoz = {0,SQRTHALF,0,SQRTHALF};
  376.     static Quat qytoz = {SQRTHALF,0,0,SQRTHALF};
  377.     static Quat qppmm = { 0.5, 0.5,-0.5,-0.5};
  378.     static Quat qpppp = { 0.5, 0.5, 0.5, 0.5};
  379.     static Quat qmpmm = {-0.5, 0.5,-0.5,-0.5};
  380.     static Quat qpppm = { 0.5, 0.5, 0.5,-0.5};
  381.     static Quat q0001 = { 0.0, 0.0, 0.0, 1.0};
  382.     static Quat q1000 = { 1.0, 0.0, 0.0, 0.0};
  383.     switch (turn) {
  384.     default: return (Qt_Conj(q));
  385.     case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break;
  386.     case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break;
  387.     case Z: qtoz = q0001; break;
  388.     }
  389.     q = Qt_Conj(q);
  390.     mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5;
  391.     mag[1] = (double)q.x*q.z-(double)q.y*q.w;
  392.     mag[2] = (double)q.y*q.z+(double)q.x*q.w;
  393.     for (i=0; i<3; i++) if (neg[i] = (mag[i]<0.0)) mag[i] = -mag[i];
  394.     if (mag[0]>mag[1]) {if (mag[0]>mag[2]) win = 0; else win = 2;}
  395.     else           {if (mag[1]>mag[2]) win = 1; else win = 2;}
  396.     switch (win) {
  397.     case 0: if (neg[0]) p = q1000; else p = q0001; break;
  398.     case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break;
  399.     case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break;
  400.     }
  401.     qp = Qt_Mul(q, p);
  402.     t = sqrt(mag[win]+0.5);
  403.     p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t));
  404.     p = Qt_Mul(qtoz, Qt_Conj(p));
  405.     } else {
  406.     float qa[4], pa[4];
  407.     unsigned lo, hi, neg[4], par = 0;
  408.     double all, big, two;
  409.     qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w;
  410.     for (i=0; i<4; i++) {
  411.         pa[i] = 0.0;
  412.         if (neg[i] = (qa[i]<0.0)) qa[i] = -qa[i];
  413.         par ^= neg[i];
  414.     }
  415.     /* Find two largest components, indices in hi and lo */
  416.     if (qa[0]>qa[1]) lo = 0; else lo = 1;
  417.     if (qa[2]>qa[3]) hi = 2; else hi = 3;
  418.     if (qa[lo]>qa[hi]) {
  419.         if (qa[lo^1]>qa[hi]) {hi = lo; lo ^= 1;}
  420.         else {hi ^= lo; lo ^= hi; hi ^= lo;}
  421.     } else {if (qa[hi^1]>qa[lo]) lo = hi^1;}
  422.     all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5;
  423.     two = (qa[hi]+qa[lo])*SQRTHALF;
  424.     big = qa[hi];
  425.     if (all>two) {
  426.         if (all>big) {/*all*/
  427.         {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);}
  428.         cycle(ka,par)
  429.         } else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
  430.     } else {
  431.         if (two>big) {/*two*/
  432.         pa[hi] = sgn(neg[hi],SQRTHALF); pa[lo] = sgn(neg[lo], SQRTHALF);
  433.         if (lo>hi) {hi ^= lo; lo ^= hi; hi ^= lo;}
  434.         if (hi==W) {hi = "\001\002\000"[lo]; lo = 3-hi-lo;}
  435.         swap(ka,hi,lo)
  436.         } else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
  437.     }
  438.     p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3];
  439.     }
  440.     k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z];
  441.     return (p);
  442. }
  443.  
  444.  
  445.  
  446.  
  447.  
  448.  
  449.  
  450.  
  451.  
  452.  
  453.  
  454. /******* Decompose Affine Matrix *******/
  455.  
  456. /* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the
  457.  * translation components, q contains the rotation R, u contains U, k contains
  458.  * scale factors, and f contains the sign of the determinant.
  459.  * Assumes A transforms column vectors in right-handed coordinates.
  460.  * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
  461.  * Proceedings of Graphics Interface 1992.
  462.  */
  463. void decomp_affine(HMatrix A, AffineParts *parts)
  464. {
  465.     HMatrix Q, S, U;
  466.     Quat p;
  467.     float det;
  468.     parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0);
  469.     det = polar_decomp(A, Q, S);
  470.     if (det<0.0) {
  471.     mat_copy(Q,=,-Q,3);
  472.     parts->f = -1;
  473.     } else parts->f = 1;
  474.     parts->q = Qt_FromMatrix(Q);
  475.     parts->k = spect_decomp(S, U);
  476.     parts->u = Qt_FromMatrix(U);
  477.     p = snuggle(parts->u, &parts->k);
  478.     parts->u = Qt_Mul(parts->u, p);
  479. }
  480.  
  481. /******* Invert Affine Decomposition *******/
  482.  
  483. /* Compute inverse of affine decomposition.
  484.  */
  485. void invert_affine(AffineParts *parts, AffineParts *inverse)
  486. {
  487.     Quat t, p;
  488.     inverse->f = parts->f;
  489.     inverse->q = Qt_Conj(parts->q);
  490.     inverse->u = Qt_Mul(parts->q, parts->u);
  491.     inverse->k.x = (parts->k.x==0.0) ? 0.0 : 1.0/parts->k.x;
  492.     inverse->k.y = (parts->k.y==0.0) ? 0.0 : 1.0/parts->k.y;
  493.     inverse->k.z = (parts->k.z==0.0) ? 0.0 : 1.0/parts->k.z;
  494.     inverse->k.w = parts->k.w;
  495.     t = Qt_(-parts->t.x, -parts->t.y, -parts->t.z, 0);
  496.     t = Qt_Mul(Qt_Conj(inverse->u), Qt_Mul(t, inverse->u));
  497.     t = Qt_(inverse->k.x*t.x, inverse->k.y*t.y, inverse->k.z*t.z, 0);
  498.     p = Qt_Mul(inverse->q, inverse->u);
  499.     t = Qt_Mul(p, Qt_Mul(t, Qt_Conj(p)));
  500.     inverse->t = (inverse->f>0.0) ? t : Qt_(-t.x, -t.y, -t.z, 0);
  501. }
  502.